#include "board.h"

//小状态机
#define  Q2_STATE_1   1
#define  Q2_STATE_2   2
#define  Q2_STATE_3   3
#define  Q2_STATE_4   4
#define  Q2_STATE_5   5

#define  Q3_STATE_1   1
#define  Q3_STATE_2   2
#define  Q3_STATE_3   3
#define  Q3_STATE_4   4
#define  Q3_STATE_5   5
#define  Q3_STATE_6   6
#define  Q3_STATE_7   7

//状态机结构体
struct state_machine State_Machine;

//初始化标志位
int q1_first_flag = 0;
int q2_first_flag = 0;
int q3_first_flag = 0;
int q4_first_flag = 0;

//圈数
int q4_circle_num = 0;

//循迹累计数
int step = 0;

//状态机初始化函数
void State_Machine_init(void)
{
	State_Machine.Main_State = STOP_STATE;
	State_Machine.Q1_State = STOP_STATE;
	State_Machine.Q2_State = STOP_STATE;
	State_Machine.Q3_State = STOP_STATE;
	State_Machine.Q4_State = STOP_STATE;
}

//任务1
void Question_Task_1(void)
{
	//任务1初始化
	if(q1_first_flag == 0)
	{
		// //角度pid初始化
		// angle_pid_init();

		//记录目标角度
		target_angle = 0;
		
		//响铃
		remind_flag = 1; 
	
		//初始化禁用
		q1_first_flag = 1;
	}

	//退出条件：检测到黑线同时满足最少里程数
	while (!(tracing_data_O != 255 && odometry_sum > 1500)) 
	{
		//跑直
		angle_control(target_angle);
	}
	
	//响铃 	
	remind_flag = 1; 

	//刹车
	stop_motor();

	//任务1初始化标志位恢复
	q1_first_flag = 0;

	//里程数清零
	odometry_sum = 0;

	//恢复暂停状态
	State_Machine.Main_State = STOP_STATE;
}

//任务2
void Question_Task_2(void)
{
	//任务2初始化
	if(q2_first_flag == 0)
	{
		//角度pid初始化
		// angle_pid_init();

		//循迹pid初始化
		// tracing_pid_init();

		//正常循迹状态
		i = 3;

		//记录目标角度
		target_angle = 0;

		//响铃
		remind_flag = 1;

		//开始问题2的小状态机
		State_Machine.Q2_State = Q2_STATE_1;

		//初始化禁用
		q2_first_flag = 1;
	}

	//问题2的状态1(跑直)
	if(State_Machine.Q2_State == Q2_STATE_1)  
	{
		//退出条件：检测到黑线同时满足最少里程数
		while (!(tracing_data_O != 255 && odometry_sum > 1000)) 
		{
			//跑直
			angle_control(target_angle);
		}
		
		//响铃
		remind_flag = 1; 

		//里程数清零
		odometry_sum = 0;

		//角度pid初始化
		// angle_pid_init();

		//进入问题2的下一个状态
		State_Machine.Q2_State = Q2_STATE_2;
	}

	// 问题2的状态2(右转灰度循迹)
	else if(State_Machine.Q2_State == Q2_STATE_2) 
	{
		//退出条件：检测不到黑线同时满足出循迹最小角度
		while (!(get_angle() < -162 && tracing_data_O == 255)) 
		{
			//循迹
		    position_control();
		}

		//循迹pid清零
		pid_change_zero(&tracing_pid);

		//响铃
		remind_flag = 1; 

		//里程数清零
		odometry_sum = 0;

		//进入问题2的下一个状态
		State_Machine.Q2_State = Q2_STATE_3;
	}

	//问题2的状态3(跑直)
	else if(State_Machine.Q2_State == Q2_STATE_3)  
	{
		//获取目标角度
		target_angle = 179.5;

		//退出条件：检测到黑线同时满足最少里程数
		while (!(odometry_sum > 1000 && tracing_data_O != 255)) 
		{
			//跑直
			angle_control(target_angle);
		}
		
		//角度pid清零
		pid_change_zero(&angle_pid);

		//响铃
		remind_flag = 1; 

		//里程数清零
		odometry_sum = 0;

		//进入问题2的下一个状态
		State_Machine.Q2_State = Q2_STATE_4;
	}

	//问题2的状态3(右转灰度循迹)
	else if(State_Machine.Q2_State == Q2_STATE_4)  
	{
		//退出条件：检测不到黑线同时满足出循迹最小角度
		while (!((get_angle() < 20 && get_angle() >0 ) &&  tracing_data_O == 255)) 
		{
			//循迹
			position_control();
		}

		//循迹pid清零
		pid_change_zero(&tracing_pid);

		//刹车
		stop_motor();
		//响铃
		remind_flag = 1; 

		//进入问题2的下一个状态
		State_Machine.Q2_State = Q2_STATE_5;
	}

	//任务结束
	else
	{
		//退出当前题目
		State_Machine.Q2_State = STOP_STATE;
		State_Machine.Main_State = STOP_STATE;

		//初始化恢复
		q2_first_flag = 0;
	}
}

//任务3
void Question_Task_3(void)
{
	//初始化
	if(q3_first_flag == 0)
	{
		// //定点转初始化
		// gyro_pid_init();

		// //循迹pid初始化
		// tracing_pid_init();

		// //角度pid初始化
		// angle_pid_init();

		//里程数清零
		odometry_sum = 0;

		//响铃
		remind_flag = 1; 

		//启动动力(定点转需要开始给一点动力才能启动)
		set_all_motor(1000, 1000);
		delay_ms(150);
		
		//开始问题3的小状态机
		State_Machine.Q3_State = Q3_STATE_1;
		
		//初始化禁用
		q3_first_flag = 1;
	}

	//问题3的状态1(定点转)
	if(State_Machine.Q3_State == Q3_STATE_1)
	{
		//定点转到斜切目标角度
		target_angle = -36;

		//退出条件：满足目标角度（误差1°）
		while (!(get_angle() >= -36.5 && get_angle() <= -35.5)) 
		{
			//定点转
			gyro_control(200, target_angle);
		}
		//定点pid清零
		pid_change_zero(&gyro_pid);

		//循迹累计清零
		step = 0;

		//响铃
		remind_flag = 1; 

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_2;
	}

	//问题3的状态2(跑直)
	else if(State_Machine.Q3_State == Q3_STATE_2)
	{
		//退出条件：检测到黑线同时满足最少里程数
		while (!(odometry_sum > 1780 && tracing_data_O != 255)) 
		{
			//跑直
			angle_control(target_angle);
		}

		//角度pid清零
		pid_change_zero(&angle_pid);

		//循迹累计清零
		step = 0;

		//声光启动
		remind_flag = 1; 

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_3;
	}

	//问题3的状态3(强制左转灰度循迹)
	else if(State_Machine.Q3_State == Q3_STATE_3)   
	{
		//退出条件：连续检测不到黑线同时满足出循迹最小角度（存在误触）
		while (!(step > 2 && ((get_angle() > 90 && get_angle() < 180) || (get_angle() > -180 && get_angle() < -90)))) 
		{
			system_time++;
			//10ms累计一次检测黑线结果
			if (system_time % 10 == 0)
			{
				if(tracing_data_O == 255 && step < 5)
				{
					step++;
				}
				else if (tracing_data_O != 255 && step > 0)
				{
					step--;
				}
			}

			//通过硬切区域后恢复正常循迹
			if (get_angle() > 50)
				i = 3;
			else
		 		i = 0;

			//循迹
			position_control();
		}

		//循迹pid清零
		pid_change_zero(&tracing_pid);

		//刹车
		stop_motor();

		//里程数清零
		odometry_sum = 0;

		//响铃
		remind_flag = 1; 

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_4;
		
	}

	//问题3的状态4(定点转)
	else if(State_Machine.Q3_State == Q3_STATE_4)
	{
		//定点转到斜切目标角度
		target_angle = -144;

		//退出条件：满足目标角度（误差1°）
		while (!(get_angle() >= -144.5 && get_angle() <= -143.5)) 
		{
			//定点转
			gyro_control(200, target_angle);
		}

		//定点pid清零
		pid_change_zero(&gyro_pid);

		//响铃
		remind_flag = 1;

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_5;
	}

	//问题3的状态5(跑直)
	else if(State_Machine.Q3_State == Q3_STATE_5)
	{
		//退出条件：检测到黑线同时满足最少里程数
		while (!(odometry_sum > 1780 && tracing_data_O != 255)) 
		{
			//跑直
			angle_control(target_angle);
		}

		//角度pid清零
		pid_change_zero(&angle_pid);

		//循迹累计清零
		step = 0;

		//声光启动
		remind_flag = 1; 

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_6;
	}

	//问题3的状态6(强制右转灰度循迹)
	else if(State_Machine.Q3_State == Q3_STATE_6)
	{
		//退出条件：连续检测不到黑线同时满足出循迹最小角度（存在误触）
		while (!(step > 2 && ((get_angle() > 0 && get_angle() < 90) || (get_angle() < 0 && get_angle() > -90)))) 
		{
			system_time++;
			//10ms累计一次检测黑线结果
			if (system_time % 10 == 0)
			{
				if(tracing_data_O == 255 && step < 5)
				{
					step++;
				}
				else if (tracing_data_O != 255 && step > 0)
				{
					step--;
				}
			}

			//通过硬切区域后恢复正常循迹
			if (get_angle() > 160)
				i = 3;
			else
				i = 1;
			
			//循迹
			position_control();
		}

			//刹车
			stop_motor();

			//循迹pid清零
			pid_change_zero(&tracing_pid);

			//里程数清零
			odometry_sum = 0;

			//响铃
			remind_flag = 1; 

			//进入问题3的下一个状态
			State_Machine.Q3_State = Q3_STATE_7;
	}

	//任务结束
	else
	{
		//退出当前题目
		State_Machine.Q3_State = STOP_STATE;
		State_Machine.Main_State = STOP_STATE;

		//任务3初始化恢复
		q3_first_flag = 0;
	}
}


void Question_Task_4(void)
{
	//初始化
	if(q4_first_flag == 0)
	{
		// //定点转初始化
		// gyro_pid_init();

		// //循迹pid初始化
		// tracing_pid_init();

		// //角度pid初始化
		// angle_pid_init();

		//里程数清零
		odometry_sum = 0;

		//响铃
		remind_flag = 1; 

		//启动动力(定点转需要开始给一点动力才能启动)
		set_all_motor(1000, 1000);
		delay_ms(150);
		
		//开始问题3的小状态机
		State_Machine.Q3_State = Q3_STATE_1;
		
		//初始化禁用
		q4_first_flag = 1;
	}

	//问题3的状态1(定点转)
	if(State_Machine.Q3_State == Q3_STATE_1)
	{
		target_angle = -38;
		if (q4_circle_num == 0) {
			target_angle = -37;
		}
		//定点转到斜切目标角度
		
		delay_ms(500);
		//退出条件：满足目标角度（误差1°）
		while (!(get_angle() >= (target_angle - 0.5) && get_angle() <= (target_angle + 0.5))) 
		{
			//定点转
			gyro_control(200, target_angle);
		}

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_2;
	}

	//问题3的状态2(跑直)
	else if(State_Machine.Q3_State == Q3_STATE_2)
	{
		//退出条件：检测到黑线同时满足最少里程数
		while (!(odometry_sum > 1780 && tracing_data_O != 255)) 
		{
			//跑直
			angle_control(target_angle);
		}

		//角度pid清零
		pid_change_zero(&angle_pid);

		//循迹累计清零
		step = 0;

		//声光启动
		remind_flag = 1; 

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_3;
	}

	//问题3的状态3(强制左转灰度循迹)
	else if(State_Machine.Q3_State == Q3_STATE_3)   
	{
		//退出条件：连续检测不到黑线同时满足出循迹最小角度（存在误触）
		while (!(step > 2 && ((get_angle() > 90 && get_angle() < 180) || (get_angle() > -180 && get_angle() < -90)))) 
		{
			system_time++;
			//10ms累计一次检测黑线结果
			if (system_time % 10 == 0)
			{
				if(tracing_data_O == 255 && step < 5)
				{
					step++;
				}
				else if (tracing_data_O != 255 && step > 0)
				{
					step--;
				}
			}

			//通过硬切区域后恢复正常循迹
			if (get_angle() > 50)
				i = 3;
			else
		 		i = 0;

			//循迹
			position_control();
		}

		//循迹pid清零
		pid_change_zero(&tracing_pid);

		//刹车
		stop_motor();

		//里程数清零
		odometry_sum = 0;

		//响铃
		remind_flag = 1; 

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_4;
		
	}

	//问题3的状态4(定点转)
	else if(State_Machine.Q3_State == Q3_STATE_4)
	{
		//定点转到斜切目标角度
		target_angle = -144;

		delay_ms(500);
		//退出条件：满足目标角度（误差1°）
		while (!(get_angle() >= -144.5 && get_angle() <= -143.5)) 
		{
			//定点转
			gyro_control(200, target_angle);
		}

		//定点pid清零
		pid_change_zero(&gyro_pid);

		//响铃
		delay_ms(500);

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_5;
	}

	//问题3的状态5(跑直)
	else if(State_Machine.Q3_State == Q3_STATE_5)
	{
		//退出条件：检测到黑线同时满足最少里程数
		while (!(odometry_sum > 1780 && tracing_data_O != 255)) 
		{
			//跑直
			angle_control(target_angle);
		}

		//角度pid清零
		pid_change_zero(&angle_pid);

		//循迹累计清零
		step = 0;

		//声光启动
		remind_flag = 1; 

		//里程计清零
		odometry_sum = 0;

		//进入问题3的下一个状态
		State_Machine.Q3_State = Q3_STATE_6;
	}

	//问题3的状态6(强制右转灰度循迹)
	else if(State_Machine.Q3_State == Q3_STATE_6)
	{
		//退出条件：连续检测不到黑线同时满足出循迹最小角度（存在误触）
		while (!(step > 2 && ((get_angle() > 0 && get_angle() < 90) || (get_angle() < 0 && get_angle() > -90)))) 
		{
			system_time++;
			//10ms累计一次检测黑线结果
			if (system_time % 10 == 0)
			{
				if(tracing_data_O == 255 && step < 5)
				{
					step++;
				}
				else if (tracing_data_O != 255 && step > 0)
				{
					step--;
				}
			}

			//通过硬切区域后恢复正常循迹
			if (get_angle() > 160)
				i = 3;
			else
				i = 1;
			
			//循迹
			position_control();
		}

			//刹车
			stop_motor();

			//循迹pid清零
			pid_change_zero(&tracing_pid);

			//里程数清零
			odometry_sum = 0;

			//响铃
			remind_flag = 1; 

			//进入问题3的下一个状态
			State_Machine.Q3_State = Q3_STATE_7;
	}

	//任务结束
	else
	{
		//圈数加1
		q4_circle_num++;

		//跑完四圈结束任务
		if(q4_circle_num >= 4)
		{
			//恢复暂停状态
			State_Machine.Main_State = STOP_STATE;

			//任务4初始化恢复
		    q4_first_flag = 0;
		}

		//继续重复任务3（不需要再初始化）
		State_Machine.Q3_State = Q3_STATE_1;

		//里程数清零
		odometry_sum = 0;
	}
}
